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ABSTRACT 


Currently  the  United  States  Navy  is  making  a  small 
footprint  in  the  world' s  littoral  regions  with  the  help  of 
the  United  States  Marine  Corps.  In  Iraq,  the  Marine  Corps 
is  actively  conducting  Riverine  operations,  however  they  are 
overly  tasked  and  in  need  of  permanent  replacement  by  the 
United  States  Navy.  In  order  to  alleviate  the  Marine  Corps, 
the  Naval  Expeditionary  Combat  Command  with  its  Riverine 
Squadrons  will  soon  take  over  these  Riverine  operational 
commitments  in  order  to  reestablish  supremacy  throughout  the 
Riverine  environment.  With  this  in  mind,  the  Chief  of  Naval 
Operations,  Center  for  Naval  Analyses  requirements.  System 
Engineering  Analysis  (SEA-11)  class  of  2007  developed  a 
concept  of  operations  (CONOPS)  which  the  Total  Ships  System 
Engineering  (TSSE)  class  of  2007  used  to  develop  a  prototype 
platform,  which  met  all  initial  design  requirements.  In 
order  to  take  full  advantage  of  this  prototype  platform, 
every  effort  was  taken  in  order  to  minimize  the  number  of 
crew  members  on  station  at  any  given  time.  The  purpose  of 
this  thesis  is  to  demonstrate  the  use  of  the  direct  method, 
which  will  allow  the  Specialized  Command  and  Control  Craft 
(SCCC)  to  conduct  a  fully  autonomous  Underway  Replenishment 
at  Sea  (UNREP)  with  a  standard  supply  vessel.  The  direct 
method  approach  allows  for  a  smooth  path  is  created  instead 
of  using  waypoint  navigation.  Additionally,  this  method 
allows  for  real-time  updates  at  (1Hz) . 
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I. 


INTRODUCTION 


A.  BACKGROUND 

When  the  end  of  the  "Cold  War"  with  the  Soviet  Union 
came  about ,  there  was  a  major  shift  in  the  United  States 
Naval  Doctrine.  With  the  United  States  Navy's  major 
opponent  on  the  high  seas  eliminated,  so  to  was  the  threat 
of  fighting  a  major  naval  battle  on  the  high  seas  as  was  the 
threat  in  years  past.  After  numerous  studies  and  analysis 
of  current  naval  operations  and  assets,  in  August  2005  the 
Chief  of  Naval  Operations  (CNO)  announced  that  the  United 
States  Navy  would  reconstitute  a  Riverine  capability, 
allowing  the  United  States  Navy  to  transition  from  a  blue 
water  navy  to  a  force  which  would  be  capable  of  sustaining 
operations  in  the  littoral  regions  of  the  world.  The  CNO' s 
vision  called  for  the  resurgence  of  the  brown  water  Riverine 
Force  which  is  called  out  in  the  CNO' s  Concept  of  Operations 
(CONOPS)  for  the  21st  Century  Riverine  Force.  This  document 
calls  for  the  formation  of  a  Naval  Expeditionary  Combat 
Command,  which  requires  a  Riverine  force  as  one  of  its 
elements.  The  primary  mission  for  this  force  is  to  conduct 
Phase  0  (shaping  and  stability)  operations,  to  provide 
maritime  security  and  to  carry  out  additional  tasks 
specifically  related  to  the  Global  War  on  Terrorism.  [1] 
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Figure  1 .  Blue  Water  to  Brown  Water  Navy 


Currently  the  United  States  Navy  is  making  a  small 
footprint  in  the  world' s  littoral  regions  with  the  help  of 
the  United  States  Marine  Corps.  In  Iraq,  the  Marine  Corps 
is  actively  conducting  Riverine  operations,  however  they  are 
overly  tasked  and  in  need  of  permanent  replacement  by  the 
United  States  Navy.  In  order  to  alleviate  the  Marine  Corps, 
the  Naval  Expeditionary  Combat  Command  with  its  Riverine 
Squadrons  will  soon  take  over  these  Riverine  operational 
commitments  in  order  to  reestablish  supremacy  throughout  the 
Riverine  environment. 

Based  on  the  Chief  of  Naval  Operations,  Center  for 
Naval  Analyses  requirements.  System  Engineering  Analysis 
(SEA-11)  class  of  2007  developed  a  concept  of  operations 
(CONOPS)  which  the  Total  Ships  System  Engineering  (TSSE) 
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class  of  2007  used  to  develop  a  prototype  platform,  which 
met  or  exceeded  all  initial  design  requirements.  The  first 
step  in  the  development  of  this  new  platform,  was  to  conduct 
a  Capability  Gap  Analysis  of  existing  Naval  assets  both  US 
and  foreign.  It  was  quickly  determined  from  this  analysis, 
that  no  current  ships  were  capable  of  fulfilling  all  of  the 
initial  requirements  requested  and  a  new  platform  would  be 
needed  to  accomplish  the  vision  of  the  CNO.  Based  off  of 
these  results,  a  functional  Element  Decomposition  of  the 
system  requirements  was  developed  and  a  preliminary  design 
was  identified.  The  ultimate  design  evolved  into  a  multi¬ 
hulled  Specialized  Command  and  Control  Craft  (SCCC) ,  which 
would  utilize  three  multi-mission  craft  (MMC)  to  accomplish 
all  mission  requirements. 

The  existing  procedure  for  conducting  Riverine 
operations  is  to  first  establish  a  land  forward  operating 
base  and  to  then  deploy  Riverine  assets  from  this  land  based 
support  center  to  carryout  various  missions.  While  Iraq  has 
shown  a  land  basing  system  to  be  effective,  in  the  future  it 
maybe  more  likely  that  the  Navy  will  require  a  sea  based 
support  structure  in  order  to  accomplish  its  Riverine 
mission.  To  accomplish  this  future  scenario,  the  Navy  could 
use  existing  assets;  however,  this  approach  limits  the 
Navy's  future  Riverine  footprint  due  to  the  limited  access 
current  assets  have  in  the  majority  of  the  rivers  of  the 
world  due  to  these  assets'  slow  speeds  and  deep  drafts.  To 
structure  the  United  States  Riverine  forces  in  such  a  way  as 
to  create  maximum  operational  flexibility  in  the  majority  of 
the  world's  littoral  regions,  this  new  platform  must  be 
produced . 
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B. 


MOTIVATION 


1 .  Manning 

The  Riverine  forces  will  comprise  of  800  personnel 
divided  among  three  squadrons .  Minus  the  command  structure 
for  the  Riverine  force,  each  squadron  allotted  225 
personnel.  Each  squadron  will  consist  of  three  SCCCs  and 
nine  MMCs .  The  TSSE  Manning  Study  resulted  in  the  need  for 
216  personnel  to  fully  man  these  ships,  with  the  remaining 
nine  comprising  the  command  structure  of  the  squadron.  The 
command  structure  will  remain  afloat  on  the  Global  Fleet 
Station  (GFS)  in  order  to  manage  the  logistics  of  the  SCCC. 
The  GFS  will  be  removed  from  the  Riverine  Area  of  Operation 
and  in  Blue  water. 


Figure  2 .  Bridge  Layout 


Every  effort  has  been  made  in  the  design  of  the 
Tiberinus  Class  to  minimize  the  number  of  crew  members  on 
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station  at  any  given  time.  With  this  in  mind,  the  bridge 
will  be  the  only  actively  manned  space  on  the  ship.  As  with 
existing  naval  ships,  the  bridge  has  been  organized  to  allow 
the  commanding  officer  to  oversee  all  aspects  of  navigation; 
however,  the  bridge  on  the  Tiberinus  Class  has  also 
incorporated  the  Combat  Information  Center  and  Damage 
Control  Central,  in  an  effort  to  centrally  locate  all 
controlling  stations.  All  bridge  watch  stations  will  be 
equipped  with  touch  screen  panels  which  will  enable  any 
watch  stander  to  reassign  their  watch  station  to  receive 
additional  information  from  any  other  watch  station  and  take 
control  from  their  console.  Figure  2  shows  a  purposed 
bridge  layout.  With  the  changes  from  traditional  naval 
watch  team  structure,  it  will  require  a  crew  of  72  personnel 
to  fully  man  the  Tiberinus  SCCC  and  the  three  associated 
MMCs . 


2 .  General 

Due  to  the  reduced  size  and  complexity  of  the  Tiberinus 
Class,  significant  advances  in  automation  of  processes  and 
procedures  had  to  be  achieved  in  order  to  allow  its  reduced 
crew  to  fully  operate  the  ship  in  efforts  to  achieve  all 
mission  objectives.  As  a  result,  all  efforts  have  been  made 
to  allow  the  ship' s  crew  to  operate  the  ship  with  minimal 
personnel  on  station. 

Currently  there  are  no  options  for  ships  to  conduct 

Underway  Replenishment  at  Sea  (UNREPS)  operations 

autonomously.  The  ability  to  accomplish  this  would  allow 

for  increased  force  flexibility  and  operation. 

Additionally,  if  not  used  for  fully  autonomously  UNREPS, 

this  technology  could  be  used  as  visual  cueing  for  complex 
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formation  maneuvers,  UNREPS,  plane  guard  operations,  or  even 
pier  dockings.  This  technology  could  be  incorporated  with  a 
heads  up  display,  which  would  use  standard  maneuvers  to 
build  a  database  of  near-optimal  trajectories  calculated 
beforehand.  These  near-optimal  trajectories  would  allow  the 
Officer  of  the  Deck  (00D)  to  not  just  mentally  visualize  the 
command,  but  this  technology  would  allow  the  00D  to  actually 
see  a  simulation  of  where  he  or  she  will  end  up,  thus  adding 
to  the  overall  situational  awareness. 

C .  SCOPE 

The  scope  of  this  thesis  will  consist  of  analytically 
developing  a  path-planning  process  which  will  generate 
trajectories  for  an  UNREP  between  a  standard  USNS  oiler  and 
the  SCCC.  The  first  step  in  achieving  this  objective  will 
be  to  develop  a  hydrodynamic  model  of  the  SCCC  in  order  to 
utilize  the  equations  of  motion  in  which  it  will  be  used  to 
simulate  the  vessel  movement. 

The  next  step  will  be  to  formulate  the  rendezvous 
trajectories  based  off  of  mathematical  basis.  With  this 
information,  the  factors  which  will  effect  the  trajectories 
shape  can  be  explained  and  constraints  can  be  formulated  to 
take  into  account  both  permissible  trajectories  and  the 
vessel  constraints.  From  here,  this  information  will  be  used 
to  generate  the  performance  index  of  the  vessel. 

After  the  trajectories  are  computed,  the  inverse 
dynamics  will  then  be  used  to  calculate  the  required  states 
of  the  vessel  at  each  point  upon  the  trajectory  path.  In 
order  to  minimize  any  violations  of  the  optimal  parameters. 
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the  values  generated  by  the  trajectory  algorithm  and  the 
inverse  dynamics  will  be  used  in  the  performance  index. 

D .  PROBLEM  FORMULATION 

The  problem  can  be  summed  up  as  follows:  The  supply 
vessel  will  provide  a  rendezvous  point.  From  this  point,  as 
an  example  they  will  suggest  a  course  of  090  degrees,  speed 
13  knots;  however,  as  is  often  the  case,  they  may  need  to 
maneuver  in  order  to  avoid  a  contact  or  to  create  a  better 
UNREP  situation.  As  the  supply  vessel  maneuvers,  they  will 
send  updates  to  the  SCCC  so  that  it  may  make  real-time 
updates  in  order  to  achieve  station  at  a  lateral  separation 
distance  of  140  ft,  while  maintaining  C090/S13kts. 

Once  this  is  achieved,  a  laser  range  find  system  will 
be  employed  to  maintain  the  lateral  separation.  This  is 
illustrated  in  the  figure  below. 
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Figure  3.  Problem  Outline 

The  figure  above  shows  a  pictorial  representation  of 
the  problem  as  described  above.  The  proposed  sequence  of 
events  is  to  have  the  supply  vessel  communicate  with  the 
SCCC  in  order  to  command  the  SCCC  to  proceed  to  the 
rendezvous  point.  The  SCCC  will  then  compute  the  necessary 
trajectory  to  complete  the  mission  and  reply  with  an 
acknowledgement  or  the  SCCC  will  decline  the  command 
request.  A  denial  from  the  SCCC  would  constitute  a 
violation  in  one  of  the  system  constraints  and  a  request  by 
the  SCCC  would  then  be  sent  in  order  to  allow  the  SCCC  to 
reach  the  rendezvous  point  by  either  altering  the  time  of 
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arrival  or  by  requesting  a  different  rendezvous  point  all 
together.  This  update  would  be  achieved  at  a  rate  of  (1  Hz) 
or  real-time. 

E.  THESIS  STRUCTURE 

The  intent  of  this  research  is  to  develop  a  direct 
method  control  for  the  SCCC  in  order  to  allow  for  an 
autonomous  UNREP.  This  will  be  conducted  by  first 
determining  the  equations  of  motion,  followed  by  the 
development  and  validation  of  the  traj ectories ^  and  finally 
by  the  vessel  simulations. 

Chapter  II  will  focus  on  the  Tiberinus  Class.  It  will 
focus  on  the  equations  of  motion  for  the  SCCC  and  the  vessel 
simulation  development.  Chapter  III  explains  the  theory  and 
equations  used  for  the  direct  method  for  rapid  prototyping. 

Chapter  IV  will  focus  on  the  development  and  validation  of 
the  trajectories  for  the  SCCC.  Chapter  V  will  present  a 
simulation  for  the  UNREP  between  the  SCCC  and  a  supply 
vessel.  Additionally f  Chapter  V  will  provide  the  thesis 
conclusions . 
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II.  THE  TIBERINUS  CLASS 


A.  SPECIALIZED  COMMAND  AND  CONTROL  CRAFT  DESCRIPTION 


The  Tiberinus  Class  Ship  has  been  designed  to  provide 
command,  control  and  support  for  Shaping  and  Stability 
operations.  In  support  of  the  previous  mentioned 
operations,  the  SCCC  will  provide  Maritime  Security  and 
carry  out  additional  tasks  specifically  related  to  the 
Global  War  on  Terrorism  (GWOT)  throughout  the  littoral 
regions  of  the  world.  By  design,  the  Tiberinus  Class  will 
provide  a  sea-based  maritime  capability  which  will  enable 
U.S.  forces  to  have  an  enhanced  presence  in  their  areas  of 
operation,  will  maintaining  the  legitimacy  and  sovereignty 
of  the  United  States'  ally  and  coalition  partners  lands. 


Figure  4 .  SCCC  and  MMC 
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This  class  has  been  designed  to  allow  for  a  Riverine 
force  to  sustain  a  forward  presence  within  a  Riverine 
environment  anywhere  in  the  world  for  an  indefinite  period 
of  time,  while  maintaining  the  capability  of  conducting 
interdiction  operations,  low  intensity  combat  operations. 
Visit  Boarding  Search  and  Seizure  (VBSS) ,  maritime  security 
operations,  and  waterborne  checkpoints. 

Each  Tiberinus  Class  Ship  has  been  fully  designed  to  be 
independent  of  each  other  and  will  provide  all  of  its  own 
hotel  services  for  its  embarked  personnel.  Although  the 
Tiberinus  Class  has  primarily  be  designed  in  a  supporting 
role,  each  ship  has  been  designed  to  allow  it  to  carryout 
the  same  missions  as  the  MMCs  in  reference  to  a  combat  role. 
The  characteristics  of  this  class  are  as  shown  in  the  table 
below . 


Characteristics 

Length  (LOA) 

135  ft 

Beam 

68  ft 

Speed 

40+  kts 

Draft 

6.2  ft 

Range  (Design) 

1,500  nm 

Range  (Maximum) 

3,750  nm 

Displacement 

(Design) 

550  LT 

Aircraft 

1  H-60  (landing,  not 
housed) 

Mission  Craft 

3  JMECs  /  MMCs 

Table  1 .  Ship  Characteristics 
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B.  EQUATIONS  OF  MOTION 


In  designing  the  model  for  this  vessel,  the  first  step 
is  to  determine  the  equations  of  motion.  Since  the  SCCC  and 
the  supply  vessel  will  be  operating  in  two  dimensions,  only 
equations  in  the  horizontal  plane  will  be  considered, 
however,  this  process  will  be  described  for  a  three 
dimensional  application  for  future  iterations.  This  is  a 
somewhat  common  equation  development  and  will  be  briefly 
described  below. 

When  dealing  with  relative  motion  certain  assumptions 
must  be  established  in  regards  to  the  motion  boundaries. 
For  the  purposes  of  this  research,  it  will  be  assumed  that 
the  vessel  will  act  as  a  rigid  body,  which  will  enable  for 
the  calculation  of  forces  and  moments  on  the  vessel. 
Additionally,  it  will  be  assumed  that  the  Earth's  rotation 
is  negligible  in  regards  to  acceleration  components  of  the 
vehicle's  center  of  mass.  This  will  allow  for  the  illusion 
that  the  vessel  will  be  moving  over  a  stationary  plane. 
Finally,  it  will  be  assumed  that  the  forces  acting  on  the 
vessel  will  have  their  origins  in  an  inertial  and/or  a 
gravitational  prospective.  Additionally,  the  other  primary 
forces  action  on  the  vessel  will  be  hydrostatic,  propulsion, 
thruster,  and  hydrodynamic  forces  from  lift  and  drag. 

For  vehicles  and  vessels  described  in  terms  of  three 
dimensional  components,  the  velocity  of  these 
vehicles/vessels  will  be  accounted  for  using  six  terms  given 
as  surge,  sway,  heave,  roll,  pitch,  and  finally  yaw.  For 
the  purpose  of  this  research,  surge  (u)  is  the  vessels 
forward  speed;  sway  (v)  is  the  side  slip  velocity,  heave  (w) 
corresponds  to  a  velocity  component  in  the  local  Z 
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direction,  however,  its  global  velocity  components  do  depend 
on  the  vessels  heading,  pitch,  and  roll.  These  three 
components  makeup  the  body  fixed  coordinate  as  shown  below. 

R0  =  T  ( a)[ui  +  vj  +  wk\  ( 2  - 1 ) 

The  other  three  terms  are  represented  of  the  angular  rate  of 
rotation  of  the  bodies  fixed  frame  co  as  shown  below. 

co  =  [pi  +  qj  +  rk]  (2-2) 

The  vector  quantity  of  these  additional  three  terms,  have 
their  defined  meanings  in  terms  of  the  vessel  motion.  The 
vessel  "roll  rate"  is  described  by  the  p  component  in 
equation  (2-2).  The  vessel  "pitch  rate"  is  described  by  the 
q  component  in  equation  (2-2) .  Finally,  the  "yaw  rate"  of 
the  vessel  is  described  by  the  r  component  as  seen  in 
equation  (2-2)  .  These  particular  components  would  be  sensed 
by  the  onboard  gyro  of  the  SCCC.  All  of  these  components 
combined,  account  for  the  overall  velocity  of  the  vessel, 
which  can  be  displayed  as  shown  below  or  as  later  displayed 
in  Chapter  III . 

x  =  \uvwpqr]T  (2-3) 

All  of  the  applied  external  loads  for  the  body 
coordinate  components  are  represented  by  the  vector 
components  of  forces  which  are  applied  on  the  body  of  the 
vessel  and  the  moments  which  are  also  applied  at  the  center 
of  the  body  fixed  frame  as  shown  below. 

v 

app 

+  ^xy0G  \  +  m{vrx.nrx.pG  +arx\)-fg  =  Yapp  (2-4) 

7 

_  ^  app  _ 
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(2-5) 


I>  +  a>  x  (l0co)  +  ffl{pGxv  +  pGxfflxv}-m? 


app 


M 


app 


N, 


app 


m  =  [*„(')  Xw(0  Zw(0  JCw(0  Mm{t)  Nm(t)f  (2-6) 


Due  to  the  SCCC  being  in  its  first  iteration  of  design, 
the  hydrodynamic  coefficients  were  computer  simulated  in 
order  to  determine  the  necessary  hydrodynamic  coefficients 
for  the  equations  of  motion  for  the  vessel.  For  the  purpose 
of  this  thesis,  the  constants  and  hydrodynamic  coefficients 
for  the  SCCC  are  not  included  due  to  the  non-trivial  nature 
of  the  task  and  the  unconventional  hull  shape  and  type. 


It  is  often  difficult  to  assess  the  vessels  mass 
moments  of  inertia  about  its  Center  of  Gravity  (CG) ,  due  to 
the  CG  change  with  loading  and  unloading  of  the  vessel. 
These  loading  and  unloading  changes  are  usually  symmetric, 
which  is  ideal  when  attempting  to  maintain  the  vessels 
proper  trim  and  heel  under  normal  static  conditions. 
Typically,  the  mass  and  angular  motion  of  the  vessel  is 
described  through  the  mass  moment  of  inertia  matrix,  which 
is  shown  below. 


L  = 


yx 


yy 


yz 


(2-7) 


The  elements  in  the  above  matrix  can  be  determined  by  the 
following  equations  below. 

4=i>m<0’2  +  z2)  (2-8) 

(  =  1 
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(2-9) 


4  =XJw<(*2+z2) 

Z=1 

L^’Zdm.tf+y2)  (2-10) 

Z=1 

N 

Ixy=Iyx=  “X  (X>’)  (2‘ 1 1 ) 

/  =  1 

4  =  4  =  -X  (xz)  (2- 1 2) 

z=l 

N 

4  =  4  =  -X  dmi  0-)  (2-13) 

1—1 

With  the  angular  velocity  vector  express  as  a  column  vector 
as  is  shown  below. 


zy  = 


P 

q 


r 


(2-14) 


the  angular  momentum  of  the  vessel  can  then  be  expressed  as: 


H0=I0w  (2-15), 


which  will  allow  one  to  utilize  Newton's  second  law  in  order 
to  achieve  the  following  equation  below. 


The  right  hand 
the  moment  of 
external  forces 


M  o  = 


da, 

dt 


’  +  Pg 


d2  R, 


m- 


dt2 


term  in  equation  (2-15) 
the  inertial  reaction 
acting  on  the  vessel. 


(2-15) 

is  representative  of 
of  the  sum  of  the 


The  vertical  plane  for  the  equations  of  motion  will 
begin  with  setting  all  horizontal  plane  variables  to  zero, 
in  order  to  allow  only  w,  q,  6 ,  and  z  to  be  the  only 
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variables  of  concern.  This,  with  the  addition  of  a  constant 
speed  and  utilizing  small  angular  changes  will  allow  one  to 
utilize  a  linear  system  approach.  The  reduced  equations  are 
shown  below. 


mwr  =  mU0q  +  ( W  -  B)  cos  0  +  8Zf  ( t ) 

(2-16) 

Iyyq  =  {ZBB-ZGW)smO  +  SZf(t) 

(2-17) 

II 

(2-18 

One  can  then  manipulate  the  above  equations  to  create  a  more 
useable  format,  which  is  shown  below  in  the  next  set  of 
equations  that  will  allow  the  user  to  conduct  matrix 
operations  and  finally  display  the  state  and  control 
matrices . 

MVK  (0  =  avxv  (0  +  bv8s(t)  (2-19) 

«-Zw  ~Zq  0 

™v  =  ~Mw  I^-M.  0  (2-20) 

0  0  1 

mU0+Z,  0 

Mq  zbB-zgW  (2-21) 

1  0 

"  Zt  " 

bv  =  MSs  (2-22) 

0 

xv(t)  =  Axv  (t)  +  BvSs(t)  (2-23) 
xv(t)  =  \yv  q  0]T  (2-24) 
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m~z *  ~Zg  0  zw  ™U0  -zq  0 

Av=  -M.  Iyy-Mq  0  Mq  zbB-zgW  (2-25) 

0  0  1 J  L°  1  0 

~m~zw  -zq  oVvzw  " 

5V=  -Mw  Iyy-Mq  0  Mw  (2-26) 

0  0  1 J  Lo 

Disregarding  the  motions  in  the  vertical  plane  will 
result  in  the  horizontal  equations  of  motions,  which  are 
shown  below. 

mvr  =-mr  +  Yivr+yvvr+Y.r  +  Yrr  +  YsSr(t)  (2-27) 

I22r  =  N.vr  +  Nvvr+  N.r  +  Nrr-  +  NsSr(t)  (2-28) 

iff  =  r  (2-29) 

As  with  the  previous  section  in  regards  to  the  vertical 
plane,  the  horizontal  dynamic  equation  is 

mhxh(t)  =  ahxh(t)  +  bhSr(t)  (2-30) 

where 

Xh(t)  =  [v  r  yff  (2-31) 

Again,  as  previously  shown,  this  equation  converted  into  a 
more  functional  form  and  the  state  and  control  matrices 
result  as  shown  below. 

xh(t)  =  Ahxh(t)  +  Bh8r(t)  (2-32) 

~m-Y,  -Y.  OT'n  Yr-mU0  O' 

A=  -N,  Izz-Nr  0  Nv  Nr  0  (2-33) 

0  0  lj  L°  1  0 
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(2-34) 


~m-Yv 

~Yr 

o' 

-1 

0q 

"S  4 

_ 1 

-N* 

4  -n> 

0 

NSr 

0 

0 

1 

0 
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III. DIRECT  METHOD  FOR  RAPID  PROTOYPING 


A.  HISTORICAL  BACKGROUND 

1 .  General  Description  of  Direct  and  Indirect  Method 

From  many  years  of  research,  it  has  been  determined 
that  the  most  precise  approach  in  solving  an  optimal  control 
problem  is  by  the  variational  approach,  which  is  based  on 
the  Pontryagin' s  minimum  principle.  This  indirect  approach 
requires  the  solution  of  the  necessary  conditions  of 
optimality  associated  with  the  infinite  dimensional  optimal 
control  problem  rather  than  optimizing  the  cost  of  a  finite 
dimensional  discretization  of  the  original  problem  directly. 
Using  this  method  does  require  advance  analytical  skill  in 
which  one  must  generate  numerical  solutions  of  the  resulting 
two-point  boundary-value  problem.  The  minimum  principle  is 
used  to  eliminate  the  controls  in  this  indirect  method 
approach,  resulting  in  a  generally  nonlinear  function  of  the 
state  and  co-state  variables.  This  indirect  approach  allows 
for  the  generation  of  benchmark  solutions,  which  will 
generally  converge  if  only  excellent  initial  guesses  for  the 
non-intuitive  con-states  are  achieved.  Additionally,  this 
requires  the  switching  structure  to  be  guessed  correctly  in 
advance . 

A  direct  method  approach  allows  for  rapid  trajectory 
prototyping  ability.  This  method  uses  finite  dimensional 
discretization  of  the  optimal  control  problem  to  a  nonlinear 
programming  problem.  While  the  direct  method  approach  does 
not  allow  for  extremely  great  precision  and  resolution  as 
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that  of  the  indirect  method,  it  does  allow  for  a  more 
practical  application  as  its  convergence  robustness  is  far 
superior . 

2 .  History 

The  ideal  of  the  direct  method  approach  was  first 
developed  by  Euler  in  the  early  1900' s,  when  he  approached 
the  solution  of  functions  as  finite  sets  of  variables.  This 
approach  was  achieved  by  representing  acceptable  functions 
in  the  form  of  infinite  power  series 

oo 

y(x)  =  E  a  kxk  ,  (3-1) 

k  =  0 

or  by  Fourier  series 

a  00 

y  ( x  )  =  — —  ^  ( a  k  cos  kx  +  b  k  sin  kx) ,  (3-2) 

2  k  =  1 

or  by  any  series  in  the  form  of 

oo 

J  O)  =  Z  ak<P  k  (x),  (3-3) 

k  =  1 

where  <pk(x)  is  a  given  function.  Thus,  instead  of  an 
infinite  series,  the  user  is  only  considering  a  finite 
series,  whose  solution  is  simply  the  function  of  a  set  of 
unknown  coefficients. 

Ritz  too  developed  a  direct  method,  in  which  his  method 

requires  a  field  problem  to  be  arranged,  in  which  it  will  be 

used  as  an  integral  minimization.  Thus,  allowing  it  to  be 

used  for  problems  which  have  variational  principles. 

Galerkin  obtained  approximate  solutions  to  boundary-value 

problems  in  a  simpler  way,  which  is  why  it  is  more  of  a 

universal  process.  When  Galerkin' s  method  is  combined  with 
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the  interpolation  equations  of  the  method  of  finite 
elements,  it  allows  one  to  solve  both  initial  and  boundary- 
value  problems,  which  Galerkin  utilized  to  solve  parabolic 
and  elliptic  partial  differential  equations. 

In  the  1950' s,  aerospace  engineers  began  to  utilize  the 
finite  element  method.  Due  to  vast  improvements  in 
computing  in  the  following  year,  this  method  became 
popularized  for  numerous  numerical  simulations  of  physical 
problems  dealing  with  stress  analysis,  structural  and  solid 
mechanics,  heat  transfer,  and  fluid  mechanics  among  others. 
Resulting  conclusions  found  that  the  previous  fore  mentioned 
methods  when  applied,  will  yield  approximation  for  the 
minima  from  above  or  the  maxima  from  below,  thus,  enabling 
the  user  to  utilize  them  for  rapid  prototyping  of  optimal 
solutions  or  near-optimal  solutions.  This  allows  for  the 
ability  to  preset  extreme  trajectories  and/or  controls, 
while  allowing  for  a  calculational  advantage,  while 
providing  a  near-optimal  solution  with  any  varying  degree  of 
accuracy . 

In  the  1960's,  Taranenko  applied  a  similar  method  to 
that  Ritz-Galerkin  to  flight  dynamics  involving  constraints 
on  states  and  controls.  Continuing  in  his  predecessors' 
methods,  he  attempted  to  use  continuous,  unequivocal  and 
differentiable  functions  which  automatically  satisfied 
boundary  conditions  of  the  function 

x,  =  xi0  +  Xj-  X—  (t - t0 )  +  O, (r),  i=  1 , . . .4  (3-4) 

^  f  ~ 

as  the  reference  function  for  the  Cartesian  coordinates  and 
speed.  In  equation  (3-4),  z  is  an  argument,  while  0;(r)  is  a 
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continuous,  unequivocal,  and  differentiable  function  which 
satisfies  the  boundary  conditions O;(r0)  =  0;(ry)  =  0  .  Taranenko 

further  suggested  the  uses  of  the  following  equations  or  any 
of  their  linear  combinations. 

<D) (r)  =  F  Ak  sin kn  — — —  (3-5) 

k= 1  Tf  ~  To 

=  <3-6) 

k= 1 

®;(r)  =  (r-ror(r-r/P  (3-7) 

While  there  are  no  actual  limitations,  one  could  use 
any  convenient  function  for  their  particular  task.  State 
parameters  and  controls  can  then  be  resolved  from  the  result 
of  their  inverse  flight  dynamics. 


Figure  5.  Splitting  Original  Interval 

In  order  to  provide  more  flexibility  without  increasing 
the  order  n  ,  Taranenko  recommended  the  splitting  of 
the  interval  [r0;zy]  into  pieces,  in  order  to  use  lower  order 
polynomials  in  order  to  describe  the  behavior  of  the  state 
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variables  x.(i  =  1,2,3, 4)  .  The  higher  order  n  (or  mx  and  m2)  ,  the 
higher  the  number  of  pieces  required  in  the  piecewise  case, 
thus  the  closer  a  near-optimal  solution  will  be  to  the 
optimal  solution. 

Taranenko  continued  his  research  with  the  hopes  of 
building  a  database  of  trajectories  for  numerous  aircraft, 
in  an  effort  to  aid  pilots  in  maneuvering  their  aircrafts, 
by  suggesting  the  maneuvers  optimal  trajectory.  Due  to  the 
lack  of  computing  speed  of  the  day,  it  was  discovered  that 
the  numerous  optimization  parameters  would  not  allow  for 
onboard  computation  of  these  trajectories. 

As  with  most  discoveries,  this  approach  was  overlooked 
until  technology  could  appropriately  catch-up  with  the 
computing  requirements.  In  1997,  Taranenko' s  dream  was 
realized  by  Yakimenko  who  tested  these  methods  onboard  a 
flying  laboratory.  Yakimenko  developed  an  algorithm  which 
computes  near-optimal  trajectories.  These  trajectories  were 
found  to  be  capable  of  satisfying  all  boundary  conditions 
prior  to  the  establishment  of  the  trajectories,  which 
allowed  for  decreased  calculational  time  and  the  ability  to 
conduct  real  time  onboard  trajectory  computation. 

B.  MATHEMATICAL  DEVELOPMENT 

In  order  to  develop  the  mathematics  for  this  problem, 
the  first  step  is  to  formulate  an  optimal  control  problem 
which  will  move  the  vessel  from  a  starting  point  (initial 
point)  to  some  final  point.  If  one  chooses  or  is  given  both 
an  initial  and  final  position,  which  will  also  serve  as  the 
problems  boundary  conditions,  a  first  order  polynomial 
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representation  of  a  trajectory  linking  both  positions  can  be 
developed  by  using  the  following  formulas: 


x(t)  =  Px  (r)  =  a0  +  axz 


(3-8) 


y(r)  =  Pv(T)  =  b0+blT 


(3-9) 


As  previously  mentioned,  T  is  given  as  any  argument. 
One  can  then  solve  for  any  unknown  coefficient  by 
substituting  any  value  for  T  and  solving  for  these  unknown 
coefficients  with  the  following  matrix  equations: 


1  0  II  a, 


1  t  \\  a 


(3-10) 


1  Olfft 


1  T\\bf  I  I  37 


(3-11) 


y  =  y0  + 


yf-yp 

Xf  -XQ 


(x-x0)  (3-12) 
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Figure  6.  Basic  First  Order  Polynomial 

The  straight  line  represented  in  the  figure  above, 
represents  the  satisfying  of  the  initial  boundary 
conditions . 

While  outlining  the  problem  of  an  UNREP  between  the 
SCCC  and  a  supply  vessel,  a  straight  line  trajectory  may  not 
be  the  optimal  trajectory  to  accomplish  this  objective,  due 
to  changes  in  course/speed,  contact  avoidance,  and  changes 
in  rendezvous  time.  Based  on  these  foreseen  events,  a 
curved  path  by  be  more  appropriate  in  achieving  this  goal. 
As  outlined  by  other  research  endeavors  and  as  previously 
mentioned  in  this  thesis,  a  curved  path  can  be  achieved  by 
breaking  the  problem  into  smaller  pieces  or  as  is  standard 
naval  practice,  put  in  additional  waypoints  to  the  final 
position  or  objective.  While  this  seems  simple  enough,  the 
added  waypoints  must  then  account  for  added  boundary 
conditions  of  that  particular  leg,  and  an  added  time  to  the 
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next  leg  must  also  be  taken  into  account.  While  this  is 
fine  for  typical  naval  applications,  it  does  add  to  the 
overall  computational  time  to  achieve  the  optimal  trajectory 
solution . 


i 

Final  Position 

1 

Waypoints 

\ 

7 

/ 

4 

1 

9 

Initial  Position 

,5 1 - ^ ^ ^ ^ ^ - 1 

-5  0  5  10  15  20  25 

X-axis  (m) 

Figure  7.  Curved  Trajectory  Using  Waypoints 


In  Figure  7,  two  waypoints  were  added  to  achieve  a 
curved  trajectory  between  the  initial  and  final  positions. 
It  is  of  interest  to  note  that  in  order  to  approximate  this 
new  path,  it  must  be  represented  as  a  polynomial. 
Additionally,  an  added  waypoint  is  a  direct  corresponding 
result  to  an  increase  in  the  order  of  the  initial 
polynomial.  In  this  case  the  corresponding  result  would  be 
a  third  order  polynomial  due  to  the  addition  of  two 
waypoints.  If  this  new  curved  path  is  represented  in  the 
form  of  a  third  order  polynomial,  it  will  produce  a  smooth 
curve  which  is  very  similar  to  the  outline  of  the  waypoints, 
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while  maintaining  the  existing  boundary  conditions.  Using  a 
direct  method  approach,  the  vessels  controls  would  be  taken 
from  the  produced  trajectory  and  the  required  states  would 
then  be  produce  by  using  inverse  dynamics. 

Assuming  the  initial  boundary  conditions  remain  the 
same,  this  third  order  polynomial  representation  of  the 
intended  trajectory  can  be  displayed  as  previously  shown 
with  only  minor  changes  as  shown  below: 


x(r)  =  Px(r)  =  YJaiTi 

i= 0 


(3-13) 


y(j)  =  Py(j)  =  YjbiTi  (3-14) 
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By  taking  into  account  the  first  derivative  of  the 
first  order  polynomial  equations,  added  path  curvature 
flexibility  will  be  increased  allowing  the  user  to  utilize 
boundary  conditions  to  gain  the  equations  for  the  velocity. 

An  example  of  the  process  for  a  single  high-order 
polynomial  approximation  for  the  coefficients  of  (A)  in  a 
"2+2"  case  are  shown  below. 

*/(ro)  =  */(°)  =  *«>  xMf)  =  xif  0-16) 

<-(To)  =  <(°)  =  *';o  x\(Tf  )  =  x'if  (3-17) 

x";(r0)  =  x'V(O)  =  x"i0  x'\(zf)  =  x"(/  ( 3-18 ) 
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(3-19) 


5 

xf(r)  =  aj0  +  anT  +  anT2  +  anT3  +  aj4T4  +  aj5T 5  =  ^ ,  a  jttk 

k= o 

5 

x  \  (r)  =  an  +  2  aj2r  +  3  aBr2  +  4  ai4r3  +  5ajSr4  =  ^  kaikrk~x  (3-20) 

k= 0 

5 

x"(.(r)  =  2an  +  6aar  +  I2ai4r2  +20  ai5T2  =  ^  k(k  -  \)aikrk~2  (3-21) 

k= 0 
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C .  HYPOTHETICAL  TRAJECTORIES 


In  order  to  create  a  random  trajectory,  the  user  must 
establish  an  initial  and  final  position,  which  will  be 
required  to  be  stationary  points.  Using  Matlab' s  Symbolic 
Toolbox,  one  can  determine  the  coefficients  of  the  example 
from  the  previous  section  as  shown  below. 
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(3-23) 


In  order  to  conduct  a  single  high-order  polynomial 
approximation  using  a  visual  check,  the  inputs  are  set  as 
below  in  the  following  table. 
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Table  2 .  Example  Inputs 


where, 

rf  =  1,,2,....,10 

xJo  =  {-0.4;  -0.1;  0.2;  0.5} 

The  resulting  plots  are  the  visual  confirmation  check  in 
meters . 


Figure  8.  Visual  Check 
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Utilizing  the  before  mentioned  techniques  above,  a 
random  trajectory  was  established  using  the  SCCC  concepts  of 
operation  to  establish  the  conditions  for  this  trajectory. 
This  random  trajectory  is  shown  below. 


Final  Position 


Inital  Position 


_ L 


X-axis  (xl )  (m) 


Figure  9.  Random  Trajectory  Generation 
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IV.  TRAJECTORY  DEVELOPMENT  AND  VALIDATION 


A.  INDENTIFIYING  CONTROL  PARAMETERS  &  CONSTRAINTS 

The  SCCC  UNREP  with  a  supply  vessel  is  a  problem 
centered  on  navigation  with  the  primary  controlled 
parameters  of  concern  being  course  and  speed,  which 
essentially  makes  this  a  two  dimensional  problem.  For  the 
propose  of  this  thesis,  the  two  vessels  in  question  will  be 
in  open  ocean  not  constrained  by  draft,  however,  the  body  of 
water  may  have  above  water  obstructions  such  as  oil 
platforms . 

Viewing  this  problem,  the  constraints  which  come  to 
mind  are  the  rudder/speed  rule  of  15/15  for  a  total  of  30 
overall.  Meaning,  one  could  use  a  speed  of  20  knots  and  a 
rudder  of  10  knots  for  a  total  of  30.  An  additional 
constraint  will  deal  with  limiting  speed  and/or  rudder 
commands  when  within  approximately  350  feet  of  the  supply 
vessel  as  to  limit  the  possibility  of  collision. 

B .  INVERSE  DYNAMICS 

Inverse  dynamics  computes  the  states  of  the 
instantaneous  position  of  the  vessel  along  the  virtual  arc 
of  a  give  trajectory.  This  process  allows  for  reverse 
engineering  of  an  executed  trajectory  in  order  to  determine 
the  required  commands  to  achieve  this  maneuver  while 
maintaining  the  necessary  boundaries  of  constraints. 

In  order  to  achieve  the  fore  mentioned  process,  it  is 
of  note  that  the  parameters  of  the  reference  trajectory  of  a 
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numerical  solution  are  calculated  in  N  points  evenly 
distributed  throughout  the  virtual  arc,  such  that 


At  = 


(4-1) 


Time  interval  between  two  points  is  calculated  as  in 
equation  (4-6) 


Ah_i  t .  t.  j  2 


^/(x.-x.-1)2+(pj-yy-1)2 
VJ  +VM 


(4-2) 


In  order  to  transition  from  a  measurement  of  position  along 
the  virtual  arc  to  that  of  velocity,  the  kinematics  for  a 
navigational  solution  must  be  achieved  and  are  shown  below 
as  an  example . 

Given: 

Xj ( t ), x2 ( t )  and  therefore  ij (/), x2 (/)  and  x,  (t), x2 (t) 

Kinematic  equations: 


dx ' 

— -  =  V  cos  T 
dt 

(4-3) 

dx„ 

— -  =  F  sin  Y 
dt 

(4-4) 

1  .  2  *2 
yXj  +x2 

(4-5) 

X, 

arctan  — 

(4-6) 

x, 


Convert  those  to  the  virtual  domain  (where  the  reference 

.  dr 

trajectories  are  developed)  using  the  virtual  speed  A=  — 

dt 


,  dx,  dx,  dt  1  T, 

Xj  = — -  =  — - —  =  — hcosT 
dr  dt  dr  A 


(4-7) 
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(4-8) 


dx ,  dx ,  dt  1  T.  .  ,T. 

-  =  —  K  sin  T 


dr  dt  dr  A 

We  will  set  a  separate  5th-order  polynomial  for  A(t)  similar 
to  that  of  equation  (3-29) 
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(4-9) 


where  A"  and  A"  being  varied  parameters  and  A0  ,  Ag ,  Af  and 


A'f  defined  as 


A0  =  V0 ,  Ag  =  0  ,  Af  =  Vf  and  A'f  =  0 


(4-10) 


Next,  to  address  the  constraints  imposed  on  the  control 
parameters  (or  in  other  words  to  account  for  the  controllers 
dynamics)  we  also  need  to  evaluate  the  following  derivatives 


t  ft  .  t  tt 


V  =  V'A  =  A'J72  +  x'2  +  A 

>  1  z  /  #o  #o 


\[x{2  +X2 


Y'r"  _  r"r' 

'P  =  V'A  =  A  12  p  2  — 2 


cos21? 


(4-11) 


(4-12) 


C.  OPTIMIZATION 

In  order  to  capitalize  on  the  ability  to  optimize  this 
problem  a  general  block  scheme  approach  is  taken  in  order  to 
accomplish  this  goal. 
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Figure  10.  General  Block-Scheme 


In  short,  Figure  10  helps  to  explain  that  this  is  a 
cycle  and  the  values  of  each  parameter  are  weighted  to 
ensure  the  errors  from  the  minimization  function  are 
continuously  calculated  until  they  are  approximately  zero, 
which  will  allow  the  UNREP  rendezvous  to  be  accomplished. 

Perforamance  Index  =  ^  Cost  Functions  +  ^  Weight  x  Penalty  (4-13) 
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Therefore  the  computational  procedure,  shown  on  Figure  10 
looks  as  follows.  We  start  from  guessing  on  varied 
parameters,  which  are  x"0 ,  x"0 ,  x"f ,  x"f  ,  A,",  and  rf  .  Then 

using  the  given  initial  and  final  conditions,  which  are 
shown  in  Table  4  below. 


Initial 

Initial 

Final 

Final 

Position  Xi 

Position  X2 

Position  xi 

Position  xi 

0 

0 

5000  yds 

5000  yds 

Table  3.  Initial  &  Final  Positions 


one  can  compute  the  coefficients  of  the  reference  functions, 
which  are  based  off  of  algebraic  polynomials  of  degree  "n" 
with  the  virtual  arc  "r"  as  the  argument.  This  allows  for 
independent  optimization  of  the  velocity  history  along  the 
trajectory.  Next  the  coefficients  of  the  reference 
functions  are  determined  using  MAT LAB .  The  user  then  makes 
an  initial  guess  on  the  values  of  the  variable  parameters. 

Using  inverse  dynamics,  the  states  and  controls  along 
the  virtual  arc  are  determined.  From  this  point  the  cost 
function  and  penalties  are  computed  and  if  these  items  are 
found  to  within  tolerance  then  the  computation  stops.  If 
the  tolerances  are  not  met,  the  values  of  the  variable 
parameters  are  changed  and  the  states  and  controls  along  the 
virtual  arc  are  recalculated  and  the  cycle  repeats  as  shown 
in  Figure  1 0 . 

Finally  we  estimate  the  performance  index  and  evaluate 
penalties,  the  penalty  weights  are  shown  in  Table  4. 
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Penalty  Parameter 

Weights 

Time 

10-3 

Sway  Velocity 

10 

Speed 

10 

Table  4 .  Penalty  Weights 


Additionally,  the  penalties  are  calculated  by  the  equations 
shown  below. 

Time  Penalty  =  (total  time  -  T)2  (4-14) 

Speed  Penalty  =  max([0, speed  -  speedmax  ])2  (4-15) 
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V. 


RESULTS  AND  CONCLUSIONS 


A.  RESULTS 

Using  the  technique  described  throughout  this  thesis, 
the  following  results  were  achieved  for  three  Specialized 
Command  and  Control  Crafts  (SCCC)  conducting  an  Underway 
Replenishment  at  Sea  with  a  standard  supply  vessel. 

For  the  three  SCCCs,  the  initial  and  final  velocities 
are  the  same,  additionally  for  two  out  of  three  of  the 
SCCCs,  the  initial  angles  are  the  same,  however  all  three 
have  the  same  final  angles.  The  change  in  one  of  the 
initial  angles  was  done  to  allow  for  a  better  visual  image 
of  the  scenario.  For  these  results  the  times  of  arrival 
were  all  the  same,  however  to  accommodate  the  supply  vessel 
all  SCCCs  would  have  varying  times  of  arrival  in  order  for 
the  supply  ship  to  connect  one  vessel  then  move  to  the  next 
one  and  so  on.  Additionally,  once  along  side,  the  ability 
for  the  real  time  updating  at  a  rate  of  (1Hz)  would  allow 
the  supply  vessel  to  alter  course  while  all  SCCCs  maintain 
their  stations. 


39 


3000 


2500 


2000 


1500 


J'1  1000 


Simultaneous  arrival  at  T  =  450  s 

l 

|_ 

k 

r 

r 

_ 

i 

1 _ 

l 

i 

1 _ 

1 _ 

i _ 

0  500  1000  1500  2000  2500  3000  3500  4000  4500 

xr  m 

Figure  11.  Multiple  SCCC  Scenario 
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Figure  12.  Close-up  of  Initial  Position  of  Multiple  Scenario 
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Figure  13.  Close-up  of  Final  Position  of  Multiple  SCCC  Scenario 
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Yaw  rate  °/s  Acceleration,  m/s 
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Figure  14.  Controls  for  First  SCCC 
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Heading,  Speed,  m/s 
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Figure  15.  States  for  First  SCCC 
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Figure  16.  Virtual  Domain  Parameters  for  First  SCCC 

B.  CONCLUSIONS 

This  model  using  the  fore  mentioned  design  method 
allows  the  problem  boundary  conditions  to  be  satisfied 
beforehand;  eliminates  wild  trajectories  which  decreases 
required  computer  computing  times;  allows  for  real  time 
updating  (1Hz)  of  the  required  outputs  due  to  the  speed  at 
which  calculations  can  be  done;  and  allows  for  the 
implementation  of  multiple  agents  for  collision-free 
motions . 

Additionally  benefits  of  this  technology  would  be  that 
it  allows  for  the  incorporation  of  a  heads  up  display  which 
could  use  standard  maneuvers  to  build  a  database  of  near- 
optimal  trajectories  calculated  beforehand.  These  near- 
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optimal  trajectories  could  all  for  the  officer  of  the  deck 
on  board  Naval  vessels  to  not  just  mentally  visualize  the 
required  commands,  but  also  allow  them  to  actually  see  a 
simulation  of  where  them  will  end  up,  thus  adding  to  the 
overall  situational  awareness.  In  principle,  if  this 
onboard  computer  was  capable  of  doing  the  required  updates 
often  enough,  the  need  for  a  traditional  feedback  controller 
would  be  unnecessary.  The  computer  would  then  be  capable  of 
continuously  regenerating  from  the  vessels  actual  position 
instead  of  fighting  the  disturbance  of  the  trajectories. 


46 


APPENDIX.  MATLAB  CODE 


A. 


STARTMESCCC.M 


%  This  is  a  main  script 

clear  all,  close  all,  clc 

syms  tf  xO  xpO  xppO  xf  xpf  xppf 


%%  Setting  the  boudary  conditions 
global  posxi  vxi  posxf  vxf 
global  posyi  vyi  posyf  vyf 
vi  =  5*0.5144;  vf  =  13*0.5144; 
posxi  =  -50;  posyi  =  60; 

posxf  =  4650;  posyf  =  2250; 

anglei  =  0;  anglef=0; 

vxi  =  vi*cos (anglei) ;  vyi  =  vi*sin (anglei) ; 
velocity 

vxf  =  vf*cos (anglef ) ;  vyf  =  vf*sin (anglef ) ; 
velocity 


%  initial  position,  m 
%  final  position,  m 

%  components  of  initial 

%  components  of  final 


%%  Defining  optimization  problem 

global  Cost_T  Fine_Speed  Fine_Accel  Fine_YawRate  Psi_dot_max  v_max  a_max 
T 

global  wv  wvd  wPsid 

R=14;  %  lateral  seperation  distance,  m 

v_max=4 0*0 . 5144 ;  %  maximum  speed,  m/s/s 

a_max=10.7;  %  maximum  acceleration,  m/s/s 

%T  =  2*sqrt ( (posxf-posxi) A2+ (posyf-posyi) A2 ) / (vi+vf ) /2 ;  % 

predetermined  time  of  arrival,  s 
T  =  550; 

Psi_dot_max  =  10*pi/180;  %  maximum  yaw  rate,  rad/s 

wv  =  10  ;  %  weighting  coefficient  for  speed 

wvd  =  10;  %  weighting  coefficient  for  accelaration 

wPsid  =  10;  %  weighting  coefficient  for  yaw  rate 


%%  Guessing  on  the  varied 
guess (1) =0 . 02*sqrt ( (posxf- 
length 

guess (2 ) =rand*0 . 01 ; 
guess ( 3 ) =rand*-0 . 0001 ; 
guess ( 4 ) =rand*-0 . 0001 ; 
guess ( 5 ) =rand*0 . 001 ; 
guess (6) =rand*-0 . 0001 ; 
guess ( 7 ) =rand*0 . 0001 ; 


parameters 

posxi) A2+ (posyf-posyi) A2) ; 


virtual  arc 


initial  acceleration  in  x 
initial  acceleration  in  y 
final  acceleration  in  x 
final  acceleration  in  y 
initial  acceleration  in  lambda 
final  acceleration  in  lambda 


%%  Defining  coefficients 
%Def ineSCCC 

%%  Compute  Coefficients 
global  a  M 

A=  [1  0  0  0  0 

0  10  0  0 


(units  are  commented) 


0; 

0; 


0 

0 


47 


0  0  1  0  0  0; 

1  tf  tf A2 / 2  tfA3/6  tfA4/12  t f A 5 / 2 0 ; 
0  1  tf  tfA2/2  tf A3/3  t f A 4 / 4 ; 

001  tf  tf A2  tf A3 ] ; 

b=[x0  xpO  xppO  xf  xpf  xppf] 
a=A\b; 

a=collect (a, tf ) ; 

M=length (a) ; 


%%  Calling  the  optimization  routine 

opt=optimset (' Display ' ,  ' iter ’  ,  'TolX',le-4,  1 TolFun 1  ,  le-4 ,  1 Maxlter 1 , 10 ) 
[guess_opt , fval ,  exit flag] =fminsearch ( 1 Tra j ectory ' , guess ,  opt) ; 

%Traj ectory (guess ) ; 


%%  Displaying  cost  function  and 
fprintf (' Cost  function 
fprintf ( '  Penalty  in  speed 
fprintf ( 1  Penalty  in  acceleration 
fprintf ( 1  Penalty  in  yaw  rate 


penalties 

%6 . 2g\n ' , Cost_T) 

%6 . 2g\n ' , Fine_Speed) 

%6 . 2g\n ' , Fine_Accel) 

%6 . 2g\n\n ' ,  Fine_YawRate) 


%%  Displaying  optimal  parameters 
%guess_opt=guess ; 


fprintf ('Arc  lenght  = 

%6 . 2f \n ' 

1 f guess  opt ( 1 ) ) 

fprintf ( ' 

initial 

accel  final  accel\n' ) 

fprintf (' along  x  : 

%6.2e 

%6 . 2e\n ' ,  [guess 

_opt  (2) 

guess  opt  ( 3 ) ] ) 

fprintf (' along  y  : 

%6.2e 

%6 . 2e\n ' ,  [guess 

_opt  ( 4 ) 

guess  opt  ( 5 ) ] ) 

fprintf (' in  lambda: 
guess  opt  ( 7 ) ] ) 

%6.2e 

%6 . 2e\n ' ,  [guess 

o 

rt 

C 

%%  Plotting  the  results 
PlotResults 


B.  TRAJECTORY . M 


function  PI=traj ectory (guess ) 

%%  This  function  computes  states  and  controls  for  the  current  guess 
global  a  M 

global  tf  xO  xpO  xppO  xf  xpf  xppf 
syms  tf  xO  xpO  xppO  xf  xpf  xppf 
global  posxi  vxi  posxf  vxf 
global  posyi  vyi  posyf  vyf 

global  t  xl  x2  v  Psi  tau  lam  v_dot  Psi_dot 


%%  Current  values 
tauf  =guess ( 1 ) ; 
accxi=guess  (2)  ; 
accyi=guess  (3) ; 
accxf=guess  (4) ; 
accyf=guess  (5) ; 
accli=guess  ( 6) ; 
acclf=guess  ( 7 )  ; 


of  varied  parameters 
%  virtual  arc  length 
%  initial  acceleration  in  x 
%  initial  acceleration  in  y 
%  final  acceleration  in  x 
%  final  acceleration  in  y 
%  initial  acceleration  in  lambda 
%  final  acceleration  in  lambda 
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%%  Defining  coordinates  in  N  nodes  in  the  virtual  domain 
al  =  subs  (a,  { ' xO '  ,  ' xpO 1 ,  ' xppO ' ,  ' xf ' ,  ' xpf ' ,  ' xppf ' ,  ' tf 1 } ,  . . . 

{posxi , vxi , accxi , posxf , vxf, accxf ,  tauf } ) ; 

a2  =  subs (a,  { ' xO ' ,  ' xpO 1 ,  1 xppO ' ,  ' xf '  ,  ' xpf 1 ,  1 xppf ' ,  ' tf ' } ,  . . . 
{posyi , vyi ,  accyi , posyf , vyf ,  accyf ,  tauf } )  ; 

a3  =  subs  (af  { ' xO ' ,  ' xpO 1 ,  1 xppO ' ,  ' xf ' ,  ' xpf 1 A  1 xppf 'tf ’ }/  •  • • 

{ 1 A  0  r accli , 1,0, acclf , tauf } )  ; 
axl=diag ([1,1, 1/2, 1/6, 1/12, 1/2  0])  *al; 
ax2=diag ([1,1, 1/2, 1/6, 1/12, 1/2  0])  ^a2; 
ax3=diag ( [1, 1, 1/2, 1/6, 1/12, 1/20] ) *a3; 

tau=linspace (0, tauf)  ; 

for  i=l : M 

cxl (i) =axl (M+l-i) ; 
cx2 (i) =ax2 (M+l-i) ; 
cx3 (i) =ax3 (M+l-i) ; 
end 

xl  =  polyval ( cxl , tau) ; 
x2  =  polyval ( cx2 , tau) ; 
lam  =  polyval (cx3, tau) ; 


%%  Defining  coordinates'  derivatives  in  N  nodes  in  the  virtual  domain 
cxl_prime  =  cxl . * [ 5 : -1 : 0 ] *eye ( 6 , 5 ) ; 
cx2_prime  =  cx2 . * [ 5 : -1 : 0 ] *eye ( 6 , 5 ) ; 
cx3_prime  =  cx3 . * [ 5 : -1 : 0 ] *eye ( 6 , 5 ) ; 
xl_prime  =  polyval (cxl_prime, tau) ; 
x2_prime  =  polyval (cx2_prime, tau) ; 
lam_prime  =  polyval (cx3_prime, tau); 


%%  Defining  coordinates'  second-order  derivatives  in  N  nodes 
exl_2prime=cxl_prime . * [ 4 : -1 : 0 ] *eye (5,4)  ; 
ex2_2prime=cxl_prime . * [ 4 : -1 : 0 ] *eye (5,4)  ; 
xl_2prime=polyval (exl_2prime, tau) ; 
x2_2prime=polyval (ex2_2prime, tau) ; 


%%  Computing  the  states  and  controls  using  Inverse  Dynamics 
N=length (xl ) ; 

del_tau  =  tauf/(N-l); 


t(l) 

=  0; 

o 

o 

time 

V  (1) 

=  sqrt (vxi 

A2+vyiA2) ; 

o 

o 

initial  speed 

Psi 

=  atan2 (x2 

prime, xl  prime); 

o 

o 

heading,  rad 

m/s 


for  j=2:N 
sq 
v  ( j  ) 
dt 
t  ( j  ) 

v_dot ( j ) 


sqrt ( (xl_prime ( j ) ) A2+ (x2_prime ( j ) ) A2 ) ; 

lam(j)^sq;  %  speed,  m/s 

2* sqrt ( (xl ( j ) -xl (j-l))A2+(x2(j) -x2 (j-l))A2)/ (v ( j ) +v ( j -1 ) ) 

t ( j  — 1 ) +dt; 

lam_prime ( j ) ^sq+ . . . 


lam ( j ) A2* ( (xl_prime ( j ) ^xl_2prime ( j ) ) + (x2_prime ( j ) ^xl_2prime ( j ) ) ) /sq; 
Psi ( j )  =  atan2 (x2_prime ( j ) , xl_prime ( j ) ) ; 
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Psi_dot  ( j ) 


=  (xl_prime  ( j  )  *x2_2prime  ( j  )  -xl_2prime  ( j  )  *x2_prime  ( j  )  )  .  .  . 
/ xl_prime (j) A2*cos  (Psi  (j) ) A2 ; 

end 

PI  =  Perf ormancelndex; 
return 


function  PI=Perf ormancelndex 

%%  This  function  computes  the  combined  performance  index 
global  t  xl  x2  v  Psi  tau  lam  v_dot  Psi_dot 

global  Cost_T  Fine_Speed  Fine_Accel  Fine_YawRate  Psi_dot_max  v_max 
T 


global  wv  wvd  wPsid 
Cost_T 
Fine_Speed 
Fine_Accel 
Fine_YawRate 
%  Fine_A 
posyfr) . A2) ) ) A2; 

PI  =  Cost_T  +  wv 
return 


=  (t (end) -T) A2; 

=  max ( [ 0 ,  abs ( v) -v_max] ) A2 ; 

=  max ( [ 0 , (abs (v_dot ) -a_max) ] ) A2 ; 

=  max ( [ 0 ,  (abs (Psi_dot ) -Psi_dot_max) ] ) A2 ; 

=  max ( 0  , R-min ( sqrt (  (xl-posxf r )  . A2+ (x2- 

Fine_Speed  +  wvd*Fine_Accel  +  wPsid*Fine_YawRate ; 


C.  PLOTRESULTS . M 


%%  This  script  plots  the  results  of  optimization 
global  t  xl  x2  v  Psi  tau  lam  v_dot  Psi_dot 

global  Cost_T  Fine_Speed  Fine_Accel  Fine_YawRate  Psi_dot_max  v_max 
T 


%%  Bird-eye  view 
f igure ( ' Name ' ,  ' 2D  View') 
title (' Optimal  Trajectory’) 
plot  (xl , x2 ) 

xlabel ( ' x_l ,  m'),  ylabel('x_2A  m') 
axis  equal,  grid  on,  hold  on 
plot(xl (1) ,x2 (1) , 'rO') 
plot  (xl (end) , x2 (end) ,  ’ rO ' ) 


%%  Plotting  time  histories 
figure ( ’Name ' ,  ' States  '  ) 
subplot  (2,1,1) 
plot ( t , v) ,  hold 

xlabel (' Time,  s'),  ylabel (' Speed,  m/s') 
plot([0  t (end) ] , v_max* [ 1  1]  ,  ' r-- ' ) 
subplot (2,1,2) 
plot  (t,Psi*180/pi) 

xlabel (' Time,  s'),  ylabel (' Heading,  Ao') 


figure ( ' Name ' , ' Controls ' ) 

subplot (2,1,1) 

plot ( t , v_dot ) ,  hold 

xlabel (' Time,  s'),  ylabel (' Acceleration,  m/sA2 ' ) 
plot([0  t (end) ] , a_max* [ 1  1 ]  ,  ' r —  '  ) 
plot  (  [ 0  t (end) ] , -a_max* [1  1 ]  ,  ' r-- ' ) 
subplot (2,1,2) 


max 


max 
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plot (t, Psi_dot*180/pi) ,  hold 
xlabel ( ' Time ,  s'),  ylabel(!Yaw  rate,  Ao/s') 
plot([0  t (end) ] , Psi_dot_max* [ 1  1 ] *180/pi , ' r-- ' ) 
plot([0  t (end) ] , -Psi_dot_max* [ 1  1 ] *180/pi, ' r-- ' ) 

f igure (' Name ',' Virtual  Domain  Parameters') 
subplot (2,1,1) 
plot  ( t , tau) 

xlabel ( ' Time,  s'),  ylabel ( ' \tau ' ) 
subplot (2,1,2) 
plot ( t , lam) 

xlabel ( 'Time,  s'),  ylabel (' \lambda,  sA{-l}') 

D.  TEST .M 

close  all 

SupplyShip  (4700,2200,0.2) 

SCCCship  (50, 0, 1) 

SCCCship (40, 90,1) 

SCCCship (-50, 60,1) 

E.  SUPPLYSHIP. M 

function  SupplyShip (X, Y, Psi ) 

%  X,Y  -  the  coordinates  of  the  ship's  center  in  the  local  tangent  plane 
in  meters 

%  Psi  -  the  orientation  of  the  ship  in  the  local  tangent  plane  in 
radians 

SCCCx=[0  250  700  754.6  700  250  0]*0.3048; 

SCCCy= [100  107  107  53.7  0  0  7]*0.3048; 

SCCCx=SCCCx-mean (SCCCx) ; 

SCCCy=SCCCy-mean (SCCCy)  ; 
len=sqrt ( SCCCx . A2  +  SCCCy . A2  )  ; 
ang=atan2 (SCCCy, SCCCx)  ; 

SCCCx=X+len . *cos (Psi-ang) ; 

SCCCy=Y+len . *sin ( Psi-ang)  ; 
patch (SCCCx, SCCCy,  ' c '  ) 
axis  equal 

F.  SCCCSHIP. M 

function  SCCCship (X, Y, Psi ) 

%  X,Y  -  the  coordinates  of  the  ship's  center  in  the  local  tangent  plane 
in  meters 

%  Psi  -  the  orientation  of  the  ship  in  the  local  tangent  plane  in 
radians 

SCCCx=[0  130  135  130  0]*0.3048; 

SCCCy= [ 68  68  34  0  0]*0.3048; 

SCCCx=SCCCx-mean (SCCCx) ; 

SCCCy=SCCCy-mean (SCCCy)  ; 
len=sqrt ( SCCCx . A2  +  SCCCy . A2  )  ; 
ang=atan2 (SCCCy, SCCCx)  ; 
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SCCCx=X+len . *cos (Psi-ang) 
SCCCy=Y+len . *sin (Psi-ang) 
patch (SCCCx, SCCCy, 'm'  ) 
axis  equal 
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